import can
import os
import sys
# 获取当前脚本的绝对路径
current_path = os.path.abspath(__file__)
# 获取脚本所在的目录
current_dir = os.path.dirname(current_path)
# 添加需要的路径
sys.path.append(os.path.join(current_dir, "..", "cybergear"))
from pcan_cybergear import CANMotorController
import threading
import signal


class TestCyberGear(object):
    def __init__(self):
        self._bus = can.Bus(interface='socketcan', channel='can0', receive_own_messages=False)
        self._motor = CANMotorController(self._bus, motor_id=127, main_can_id=254)
        self._motor.write_single_param("spd_ref", value=0)
        self._motor.enable()
        self._motor.write_single_param("run_mode", value=2)
        self._input_thread = threading.Thread(target=self.set_param, name="set_param")
        self._input_thread.start()
        signal.signal(signal.SIGINT, self.quit)

    def set_param(self):
        while True:
            p = input("input control param:")
            t = p.split(',')
            self._motor.write_single_param(t[0], value=float(t[1]))

    def quit(self, signum, frame):
        self._motor.write_single_param("spd_ref", value=0)
        self._motor.disable()
        sys.exit()

t = TestCyberGear()
